import cv2
from matplotlib import pyplot as plt
import numpy as np
from skimage import filters

'''通过3个关键点(landmarks[22,23,28])，根据直线和垂线，计算斜矩形4个角点的坐标(A,B,C,D)'''
def cal_forthROILandmark(landmark_a,landmark_b,landmark_c):
    '''
    np方程组求解 参考 https://blog.csdn.net/sinat_41696687/article/details/109993517
    :param landmark_a: landmarks[22]  (x1,y1)
    :param landmark_b: landmarks[23]  (x2,y2)
    :param landmark_c: landmarks[28]  (x3,y3)
    :return: 求解出斜矩形的四个坐标点A,B,C,D, type = list()

    目的是求解出：
    1）平行于ab，过点c的直线ab‘；
    2）平行于ab，过点d的直线ab’‘；
    3）平行与cd，过点a的直线cd’
    4）平行与cd，过点b的直线cd’’
    进而两两联立上面的方程，求解出斜矩形的四个坐标点A,B,C,D
    '''
    x1, y1 = landmark_a
    x2, y2 = landmark_b
    x3, y3 = landmark_c

    '''
    Step1：计算ab，cd交点o，=> 得到cd直线方程co
        根据两点式方程，化简ab直线方程: y=(y2-y1)/(x2-x1) * x - (x1(y2-y1) - y1(x2-x1))/(x2-x1)
        计算ab直线的垂线方程cd: y = -(x2-x1)/(y2-y1) * x + b，代入c点化简得b = y3 + (x2-x1)/(y2-y1) * x3
        通过联立方程组：
            [(y2-y1)/(x2-x1) * x - y] = (x1(y2-y1) - y1(x2-x1))/(x2-x1)   （1）
            [(x2-x1)/(y2-y1) * x + y] = y3 + (x2-x1)/(y2-y1) * x3     （2）
        计算ab，cd的交点
    '''
    # P1 = np.array([[(y2-y1)/(x2-x1),-1],[(x2-x1)/(y2-y1), 1]])
    # P2 = np.array([(x1 * (y2-y1) - y1 * (x2-x1))/(x2-x1), y3 + (x2-x1)/(y2-y1) * x3])
    # X = np.linalg.inv(P1).dot(P2)  #ab与直线cd的交点o
    # print(f"直线ab与直线cd的交点O坐标为{X[0],X[1]}")

    '''
    Step2：计算co的距离
        根据点c到直线ab的距离公式: s = |(y2-y1)/(x2-x1) * x3 - y3 - (x1(y2-y1) - y1(x2-x1))/(x2-x1)| / sqrt((y2-y1)/(x2-x1)^2 + 1)
        求解c点到ab直线的距离dist
    '''
    dist = np.fabs((y2-y1)/(x2-x1) * x3 - y3 - (x1 * (y2-y1) - y1 * (x2-x1))/(x2-x1)) / np.sqrt((y2-y1)/(x2-x1)**2 + 1)

    '''
    Step3：求解平行于ab，过点c的直线ab‘ 
        设平行与ab的直线方程ab‘为 y=(y2-y1)/(x2-x1) * x - z,
        代入c点，得到z = (y2-y1)/(x2-x1) * x3 - y3
        即ab’方程为 y=(y2-y1)/(x2-x1) * x - ((y2-y1)/(x2-x1) * x3 - y3)
    '''
    z = (y2-y1)/(x2-x1) * x3 - y3  #ab‘方程中的常数项

    '''
    Step4：求解平行于ab，过点d的直线ab‘’ 
        设ab‘’直线方程:  y = (y2-y1)/(x2-x1) * x - u
        计算c点到ab‘’的距离: S = |(y2-y1)/(x2-x1) * x3 - y3 - u| / sqrt((y2-y1)/(x2-x1)^2 + 1) = 2 * dist
        化简S，得到 u = (y2 - y1)/(x2-x1) * x3 - y3 +/- sqrt((y2-y1)/(x2-x1)^2 + 1) * 2 * dist
        进而得到ab‘’直线方程
    '''
    #ab‘’方程的常数项（判断u1，u2哪个合适，可以根据常识，d点y4一定小于c点y3）
    u1 = (y2 - y1) / (x2 - x1) * x3 - y3 + np.sqrt((y2 - y1) / (x2 - x1) ** 2 + 1) * 2 * dist
    u2 = (y2 - y1) / (x2 - x1) * x3 - y3 - np.sqrt((y2 - y1) / (x2 - x1) ** 2 + 1) * 2 * dist
    #比较ab‘’与ab的截距，即选择u1,u2中最大的
    u = max(u1,u2)

    '''
    Step5：求解平行于cd，过点a的直线cd‘ (cd‘垂直于ab)
        设cd‘直线方程:  y = -(x2-x1)/(y2-y1) * x - v
        代入a点(x1,y1)，y1 = -(x2-x1)/(y2-y1) * x1 - v
        得到v = -(x2-x1)/(y2-y1) * x1 - y1
        进而得到cd'方程
    '''
    A, B, C, D = [],[],[],[]

    # landmark_a = (293, 298), landmark_b = (330, 298), landmark_c = (312, 332)
    # y2 - y1 = 298 - 298 = 0
    if(((y2 - y1) * x1 == 0) or ((y2 - y1) * x2 == 0)):  #y2 = y1,方程求解失效，这里注意纵轴为y，横轴为x（和以往的x表示height，y表示width不同）

        # print(f"landmark_a = {landmark_a}, landmark_b = {landmark_b}, landmark_c = {landmark_c}")
        # print(f"y2 - y1 = {y2}  - {y1} = {y2 - y1}")

        dist = y3 - y2
        temp = y2 - dist
        if(temp < 0):
            A = np.array([x1,0])
            B = np.array([x2,0])
        else:
            A = np.array([x1, temp])
            B = np.array([x2, temp])

        C = np.array([x1,y3])
        D = np.array([x2,y3])

        return A, B, C, D

    v = -(x2 - x1) / (y2 - y1) * x1 - y1

    '''
    Step6：求解平行于cd，过点b的直线cd‘‘ 
        设cd‘‘直线方程:  y = -(x2-x1)/(y2-y1) * x - w
            代入b点(x2,y2)，y2 = -(x2-x1)/(y2-y1) * x2 - w
            得到w = -(x2-x1)/(y2-y1) * x2 - y2
            进而得到cd'方程
    '''
    w = -(x2 - x1) / (y2 - y1) * x2 - y2

    '''
    联立ab’和cd‘，求解A
        ab’: y = (y2-y1)/(x2-x1) * x - z
        cd‘: y = -(x2-x1)/(y2-y1) * x - v
    '''
    M1 = np.array([[(y2-y1) / (x2-x1), -1], [(x2 - x1) / (y2 - y1), 1]])
    M2 = np.array([ z, -v])
    A = np.linalg.inv(M1).dot(M2)  #ab‘与直线cd’的交点A(前提是M1有逆)
    # print(f"直线ab‘与直线cd’的交点A: (x = {A[0]},y = {A[1]})")

    '''
    联立ab’和cd’‘，求解B
        ab’: y = (y2-y1)/(x2-x1) * x - z
        cd‘’: y = -(x2-x1)/(y2-y1) * x - w
    '''
    M1 = np.array([[(y2 - y1) / (x2 - x1), -1], [(x2 - x1) / (y2 - y1), 1]])
    M2 = np.array([z, -w])
    B = np.linalg.inv(M1).dot(M2)  # ab‘与直线cd’‘的交点B
    # print(f"直线ab‘与直线cd’’的交点B: (x = {B[0]},y = {B[1]})")

    '''
    联立ab’’和cd‘，求解C
        ab’’: y = (y2-y1)/(x2-x1) * x - u
        cd‘: y = -(x2-x1)/(y2-y1) * x - v
    '''
    M1 = np.array([[(y2 - y1) / (x2 - x1), -1], [(x2 - x1) / (y2 - y1), 1]])
    M2 = np.array([u, -v])
    C = np.linalg.inv(M1).dot(M2)  # ab‘‘与直线cd’的交点C
    # print(f"直线ab‘‘与直线cd’的交点C: (x = {C[0]},y = {C[1]})")

    '''
    联立ab‘’和cd‘‘，求解D
        ab’’: y = (y2-y1)/(x2-x1) * x - u
        cd‘’: y = -(x2-x1)/(y2-y1) * x - w
    '''
    M1 = np.array([[(y2 - y1) / (x2 - x1), -1], [(x2 - x1) / (y2 - y1), 1]])
    M2 = np.array([u, -w])
    D = np.linalg.inv(M1).dot(M2)  # ab‘‘与直线cd’’的交点D
    # print(f"直线ab‘‘与直线cd’’的交点D: (x = {D[0]},y = {D[1]})")

    '''返回的坐标位置如下:
        [C  A]
        [D  B]
    '''
    return A,B,C,D

'''不规则ROI提取 参考 https://blog.csdn.net/lyxleft/article/details/90675666 '''
def irregularROI_extract(img, ROI_locPoints):
    '''
    :param img: type=ndarray
    :param ROI_locPoints: type=list, [[x1,y1],[x2,y2]...]
    :return: ROI区域图片
    '''

    global src, ROI, ROI_flag, mask2
    mask = np.zeros(img.shape, np.uint8)
    pts = np.array([ROI_locPoints], np.int32)  # pts是多边形的顶点列表（顶点集）
    pts = pts.reshape((-1, 1, 2))
    # 这里 reshape 的第一个参数为-1, 表明这一维的长度是根据后面的维度的计算出来的。
    # OpenCV中需要先将多边形的顶点坐标变成顶点数×1×2维的矩阵，再来绘制

    # --------------画多边形---------------------
    mask = cv2.polylines(mask, [pts], True, (255, 255, 255))
    ##-------------填充多边形---------------------
    mask2 = cv2.fillPoly(mask, [pts], (255, 255, 255))
    # cv2.imshow('mask', mask2)
    # cv2.imwrite('mask.jpg', mask2)
    contours, hierarchy = cv2.findContours(cv2.cvtColor(mask2, cv2.COLOR_BGR2GRAY), cv2.RETR_TREE,
                                                  cv2.CHAIN_APPROX_NONE)
    ROIarea = cv2.contourArea(contours[0])
    # print("ROIarea:", ROIarea)

    ROI = cv2.bitwise_and(mask2, img)

    #将ROI黑色区域填充上ROI区域的像素均值

    # cv2.imwrite('ROI.jpg', ROI)
    # cv2.imshow('ROI', ROI)
    return ROI

'''通过ROI区域的4个角点和处理过的图片，将不规则的ROI裁剪成正规矩形'''
def crop_ROI(img,A,B,C,D):
    '''
    :param img: 经过ROI提取的，边缘部分用黑色填充的要裁剪的图片
    :param A: 角点1
    :param B: 角点2
    :param C: 角点3
    :param D: 角点3
    :return:经过裁剪的ROI，和要计算旋转角的两个向量
    '''
    x_min,y_min = np.min(np.array([A,B,C,D]),axis=0)  #求各列的最小值
    x_max,y_max = np.max(np.array([A,B,C,D]),axis=0)  #求各列的最大值
    ROI = img[round(y_min) : round(y_max), round(x_min) : round(x_max),:]
    return ROI

#将ROI黑色区域填充上ROI区域的像素均值
def irregularROI_extract_and_crop(img, ROI_locPoints):
    '''
    :param img: type=ndarray
    :param ROI_locPoints: type=list, [[x1,y1],[x2,y2]...]
    :return: ROI区域图片
    '''
    A, B, C, D = ROI_locPoints
    x_min, y_min = np.min(np.array([A, B, C, D]), axis=0)  # 求各列的最小值
    x_max, y_max = np.max(np.array([A, B, C, D]), axis=0)  # 求各列的最大值
    padding_b = np.mean(img[round(y_min): round(y_max), round(x_min): round(x_max), 0])
    padding_g = np.mean(img[round(y_min): round(y_max), round(x_min): round(x_max), 1])
    padding_r = np.mean(img[round(y_min): round(y_max), round(x_min): round(x_max), 2])

    global src, ROI, ROI_flag, mask2
    mask = np.zeros(img.shape, np.uint8)
    pts = np.array([ROI_locPoints], np.int32)  # pts是多边形的顶点列表（顶点集）
    pts = pts.reshape((-1, 1, 2))
    # 这里 reshape 的第一个参数为-1, 表明这一维的长度是根据后面的维度的计算出来的。
    # OpenCV中需要先将多边形的顶点坐标变成顶点数×1×2维的矩阵，再来绘制

    # --------------画多边形---------------------
    mask = cv2.polylines(mask, [pts], True, (255, 255, 255))
    ##-------------填充多边形---------------------
    mask2 = cv2.fillPoly(mask, [pts], (255, 255, 255))
    # cv2.imshow('mask', mask2)
    # cv2.imwrite('mask.jpg', mask2)
    contours, hierarchy = cv2.findContours(cv2.cvtColor(mask2, cv2.COLOR_BGR2GRAY), cv2.RETR_TREE,
                                           cv2.CHAIN_APPROX_NONE)
    ROIarea = cv2.contourArea(contours[0])
    # print("ROIarea:", ROIarea)

    #ROI区域提取
    ROI = cv2.bitwise_and(mask2, img)
    #ROI区域裁剪
    ROI = ROI[round(y_min): round(y_max), round(x_min): round(x_max), :]
    # 将ROI黑色区域填充上ROI区域的像素均值
    for i in range(len(ROI)):
        for j in range(len(ROI[0])):
            if(ROI[i][j][0] == 0):
                ROI[i][j][0] = padding_b
            if (ROI[i][j][1] == 0):
                ROI[i][j][1] = padding_g
            if (ROI[i][j][2] == 0):
                ROI[i][j][2] = padding_r

    # cv2.imshow('ROI', ROI)
    return ROI

'''提眉检测（BAR）'''
def browRaised_detect(landmark,threshold):
    '''
    @:param landmark: ndarray [(),(),...,]
    @:param threshold: 提眉检测阈值
    @:return BAR,isRaised
    BAR计算公式  BAR = \frac{|p_{20} - p_{42}| + |p_{21} - p_{41}|}{2 \times |p_{22} - p_{18}|}
    '''
    isRaised = False
    BAR = (np.fabs(landmark[20][1] - landmark[42][1]) + np.fabs(landmark[21][1] - landmark[41][1])) / (2 * np.fabs(landmark[22][0] - landmark[18][0]))
    if(BAR > threshold):
        isRaised = True
    return BAR,isRaised

'''关于距离一阶差分绘制折线图'''
def BAR_linePlot(x_list,y_list):

    plt.clf()  # 可以避免重复出现标签
    # 动态绘制折线图
    # x_list.append(frame_count)
    # d_list.append(delta_d)
    # plt.subplot(2, 1, 1)
    plt.xlabel("frame_count")
    plt.plot(x_list, y_list, c='b', ls='-', marker=',', label="BAR取值")  ## 保存历史数据
    plt.legend(loc="upper left")
    plt.pause(0.1)
    pass

#彩色直方图均衡化 - 增加图像对比度
def rgb_equalizeHist(img):

    '''
    :param img: 原始RGB图像
    :return: equ: 彩色直方图均衡化图像
    '''
    '''彩色直方图均衡化'''
    b, g, r = cv2.split(img)
    equ_b = cv2.equalizeHist(b)
    equ_g = cv2.equalizeHist(g)
    equ_r = cv2.equalizeHist(r)
    equ = cv2.merge((equ_b, equ_g, equ_r))
    return equ

#灰度直方图均衡化 - 增加图像对比度
def gray_equalizeHist(img):

    '''
    :param img: 原始RGB图像
    :return: equ: 彩色直方图均衡化图像
    '''
    '''灰度直方图均衡化'''
    equ = cv2.equalizeHist(img)
    # cv2.imshow("equ",equ)
    return equ

#brow ROI 进行Canny边缘检测
def canny_edge_detect(img,apertureSize):
    '''
    :param img: 灰度图像
    :param apertureSize: 算子孔径大小
    :return: edges图像
    '''
    threshold1 = 250  # minVal
    threshold2 = 255  # maxVal
    edges = cv2.Canny(img, threshold1=threshold1, threshold2=threshold2, L2gradient=True, apertureSize=apertureSize)
    return edges

#brow ROI 进行Laplacian滤波
def laplacian_filter(img,kernelSize):
    '''
    :param img: 灰度图像
    :param kernelSize: 算子模板大小
    :return:
    '''
    laplacian = cv2.Laplacian(img, cv2.CV_64F, ksize=kernelSize)
    laplacian = cv2.convertScaleAbs(laplacian)
    dst = img - laplacian
    return dst

#sobel梯度算子提取边缘
def sobel_edge_extract(image):
    '''
    :param image:
    :return: 返回颜色空间为[0,255]的ndarray
    '''
    edges = filters.sobel(image)  #skimage像素范围为[0,1]，cv2像素范围为[0,255]
    edges = np.uint8(edges * 255)  #转成cv2 image
    return edges

'''皱眉图像处理：中值滤波 + 直方图均衡化 + canny边缘提取/sobel边缘提取'''
def brow_process(brow_img, PR_MODE=False):

    '''
    :param brow_img: 眉毛ROI区域(BGR)
    :param CANNY: True表示使用canny边缘检测，否则使用sobel检测
    :return: 返回BGR格式的眉毛边缘图片
    '''
    #眉毛ROI转灰色
    brow_img = cv2.cvtColor(brow_img,cv2.COLOR_BGR2GRAY)
    # 图片平滑，去除噪声（这里使用中值滤波）
    # brow_img = cv2.medianBlur(brow_img, 5)  # 高斯模糊

    brow_img = gray_equalizeHist(brow_img)  # 灰色直方图均衡化

    if(PR_MODE):  #canny边缘检测
        edge = canny_edge_detect(brow_img, apertureSize=5)  # canny边缘检测(apertureSize: 3-7奇数）
    else: #sobel边缘检测
        edge = sobel_edge_extract(brow_img)

    # edge = cv2.adaptiveThreshold(edge, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY, 5, 3)
    edge = cv2.cvtColor(edge, cv2.COLOR_GRAY2BGR)
    return edge
